// CVisUtility.cpp
// Developed by Yang Yu (yuy@janelia.hhmi.org), Sept 25, 2012
// 

#ifndef __CVISUTILITY_HPP
#define __CVISUTILITY_HPP

#include "CVisUtility.h"

//-------------------
/// basic functions
//-------------------

// max
template <class T>
T y_max(T x, T y)
{
    return (x>y)?x:y;
}

// min
template <class T>
T y_min(T x, T y)
{
    return (x<y)?x:y;
}

// abs
template <class T>
T y_abs(T x)
{
    return (x<(T)0)?-x:x;
}

// delete 1d pointer
template <class T>
void y_del(T *&p)
{
    if(p) {delete []p; p=NULL;}
    return;
}

// delete 2d pointers
template<class Tdata, class Tidx>
void y_del2d(Tdata **&p, Tidx column)
{
    if(p)
    {
        for(Tidx i=0; i<column; i++)
        {
            delete[] p[i];
        }
        delete []p; p=NULL;
    }
    return;
}

// delete 2 1d pointers
template<class T1, class T2>
void y_del2(T1 *&p1, T2 *&p2)
{
    y_del<T1>(p1);
    y_del<T2>(p2);
    return;
}

// new 1d pointer
template<class T, class Tidx>
void y_new(T *&p, Tidx N)
{
    //
    y_del<T>(p);

    //
    try
    {
        p = new T [N];
    }
    catch(...)
    {
        cout<<"Attempt to allocate memory failed!"<<endl;
        y_del<T>(p);
        return;
    }
    return;
}

// sign
template<class T>
T y_sign(T x)
{
    return ((x>=(T)0) ? (T)1 : (T)(-1));
}

template<class T>
T y_sign2v(T a, T b)
{
    return (y_sign<T>(b)*y_abs<T>(a));
}

// quick sort
template <class Tdata, class Tidx>
void quickSort(Tdata *a, Tidx l, Tidx r, Tidx N)
{
    //
    /// sort by descent order
    //
    
    if(l>=r) return;
    Tidx i=l;
    Tidx j=r+1;

    Tdata pivot = a[l];

    while(true)
    {
        do{ i = i+1; } while(a[i]>pivot);
        do{ j = j-1; } while(a[j]<pivot);

        if(i>=j) break;

        //swap
        Tdata tmp;
        tmp = a[i]; a[i] = a[j]; a[j] = tmp; //
    }

    //
    a[l] = a[j];
    a[j] = pivot;

    //
    quickSort(a, l, j-1, N);
    quickSort(a, j+1, r, N);
}

// dist
template<class T>
T y_dist(T x1, T y1, T z1, T x2, T y2, T z2)
{
    return (sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2) ));
}


//------------------------------
/// Image Processing Functions
//------------------------------

template<class Tdata, class Tidx>
void HistogramLUT<Tdata, Tidx> :: initLUT(Tdata *p, Tidx sz, Tidx nbins)
{
    //
    if(!p || nbins<=0)
    {
        cout<<"Invalid inputs"<<endl;
        return;
    }

    //
    bins = nbins;
    y_new<Tdata, Tidx>(lut, bins);

    // histogram bin #i [minv+i*stepv, minv+(i+1)*stepv)
    minv=INF;
    maxv=-INF;
    REAL stepv=0;
    for(Tidx i=0; i<sz; i++)
    {
        if(minv>p[i]) minv=p[i];
        if(maxv<p[i]) maxv=p[i];
    }
    stepv = (maxv - minv)/(REAL)bins;

    for(Tidx i=0; i<bins; i++)
    {
        lut[i] = minv + i*stepv; // only left values
    }

    return;
}

template<class Tdata, class Tidx>
Tidx HistogramLUT<Tdata, Tidx> :: getIndex(Tdata val)
{
    //
    Tidx min=0;
    Tidx max=bins-1;

    // binary search
    bool found = false;
    index=0;
    while(min<max && !found)
    {
        Tidx mid=(min+max)/2;

        if(val == lut[mid] || (val > lut[mid] && y_abs<Tdata>(val - lut[mid])<EPS) ) // =
        {
            found = true;
            index = mid;
        }
        else if(val < lut[mid]) // <
        {
            max = mid - 1;
        }
        else // >
        {
            if(mid+1>=max)
            {
                found = true;
                index = mid;
            }
            else
            {
                min = mid + 1;
            }
        }
    }

    //
    return index;
}

/*** start c codes for CLAHE ***/
//ANSI C codes from the article "Contrast Limited Adaptive Histogram Equalization"
// by Karel Zuiderveld (karel@cv.ruu.nl) in "Graphics Gems IV", Academic Press, 1994
// modified to templates by Yang Yu (yuy@janelia.hhmi.org) 05/02/2012
template <class Tdata, class Tidx>
int CLAHE2D (Tdata* pImage, Tidx uiXRes, Tidx uiYRes,
             Tdata Min, Tdata Max, Tidx uiNR_OF_GREY, Tidx uiNrX, Tidx uiNrY,
             Tidx uiNrBins, REAL fCliplimit)
/*   pImage - Pointer to the input/output image
 *   uiXRes - Image resolution in the X direction
 *   uiYRes - Image resolution in the Y direction
 *   Min - Minimum greyvalue of input image (also becomes minimum of output image)
 *   Max - Maximum greyvalue of input image (also becomes maximum of output image)
 *   uiNR_OF_GREY > Max - for 8-bit data is 256, 12-bit data is 4096
 *   uiNrX - Number of contextial regions in the X direction (min 2, max uiMAX_REG_X)
 *   uiNrY - Number of contextial regions in the Y direction (min 2, max uiMAX_REG_Y)
 *   uiNrBins - Number of greybins for histogram ("dynamic range")
 *   fCliplimit - Normalized cliplimit (higher values give more contrast)
 * The number of "effective" greylevels in the output image is set by uiNrBins; selecting
 * a small value (eg. 128) speeds up processing and still produce an output image of
 * good quality. The output image will have the same minimum and maximum value as the input
 * image. A clip limit smaller than 1 results in standard (non-contrast limited) AHE.
 */
{
    unsigned int uiX, uiY;		  /* counters */
    unsigned int uiXSize, uiYSize, uiSubX, uiSubY; /* size of context. reg. and subimages */
    unsigned int uiXL, uiXR, uiYU, uiYB;  /* auxiliary variables interpolation routine */
    Tidx ulClipLimit, ulNrPixels;/* clip limit and region pixel count */
    Tdata* pImPointer;		   /* pointer to image */
    Tdata *aLUT=NULL;	    /* lookup table used for scaling of input image */
    Tidx* pulHist, *pulMapArray=NULL; /* pointer to histogram and mappings*/
    Tidx* pulLU, *pulLB, *pulRU, *pulRB; /* auxiliary pointers interpolation */

    y_new<Tdata, Tidx>(aLUT, uiNR_OF_GREY);

    Tidx uiMAX_REG_X = uiXRes; // dimx
    Tidx uiMAX_REG_Y = uiYRes; // dimy

    if (uiNrX > uiMAX_REG_X) return -1;	   /* # of regions x-direction too large */
    if (uiNrY > uiMAX_REG_Y) return -2;	   /* # of regions y-direction too large */
    if (uiXRes % uiNrX) return -3;	  /* x-resolution no multiple of uiNrX */
    if (uiYRes % uiNrY) return -4;	  /* y-resolution no multiple of uiNrY */
    if (Max >= uiNR_OF_GREY) return -5;	   /* maximum too large */
    if (Min >= Max) return -6;		  /* minimum equal or larger than maximum */
    if (uiNrX < 2 || uiNrY < 2) return -7;/* at least 4 contextual regions required */
    if (fCliplimit == 1.0) return 0;	  /* is OK, immediately returns original image. */
    if (uiNrBins == 0) uiNrBins = 128;	  /* default value when not specified */

    y_new<Tidx, Tidx>(pulMapArray, uiNrX*uiNrY*uiNrBins);

    uiXSize = uiXRes/uiNrX; uiYSize = uiYRes/uiNrY;  /* Actual size of contextual regions */
    ulNrPixels = uiXSize * uiYSize;

    if(fCliplimit > 0.0) {		  /* Calculate actual cliplimit	 */
        ulClipLimit = fCliplimit * (uiXSize * uiYSize) / uiNrBins;
        ulClipLimit = (ulClipLimit < 1UL) ? 1UL : ulClipLimit;
    }
    else ulClipLimit = 1UL<<14;		  /* Large value, do not clip (AHE) */
    MakeLut2D<Tdata, Tidx>(aLUT, Min, Max, uiNrBins);	  /* Make lookup table for mapping of greyvalues */

    /* Calculate greylevel mappings for each contextual region */
    for (uiY = 0, pImPointer = pImage; uiY < uiNrY; uiY++) {
        for (uiX = 0; uiX < uiNrX; uiX++, pImPointer += uiXSize) {

            pulHist = &pulMapArray[uiNrBins * (uiY * uiNrX + uiX)];

            MakeHistogram2D<Tdata, Tidx>(pImPointer,uiXRes,uiXSize,uiYSize,pulHist,uiNrBins,aLUT);
            ClipHistogram2D<Tidx>(pulHist, uiNrBins, ulClipLimit);
            MapHistogram2D<Tdata, Tidx>(pulHist, Min, Max, uiNrBins, ulNrPixels);
        }

        pImPointer += (uiYSize - 1) * uiXRes;		  /* skip lines, set pointer */
    }

    /* Interpolate greylevel mappings to get CLAHE image */
    for (pImPointer = pImage, uiY = 0; uiY <= uiNrY; uiY++)
    {
        //
        if (uiY == 0) {					  /* special case: top row */
            uiSubY = uiYSize >> 1;  uiYU = 0; uiYB = 0;
        }
        else {
            if (uiY == uiNrY) {				  /* special case: bottom row */
                uiSubY = uiYSize >> 1;	uiYU = uiNrY-1;	 uiYB = uiYU;
            }
            else {					  /* default values */
                uiSubY = uiYSize; uiYU = uiY - 1; uiYB = uiYU + 1;
            }
        }
        for (uiX = 0; uiX <= uiNrX; uiX++) {
            if (uiX == 0) {				  /* special case: left column */
                uiSubX = uiXSize >> 1; uiXL = 0; uiXR = 0;
            }
            else {
                if (uiX == uiNrX) {			  /* special case: right column */
                    uiSubX = uiXSize >> 1;  uiXL = uiNrX - 1; uiXR = uiXL;
                }
                else {					  /* default values */
                    uiSubX = uiXSize; uiXL = uiX - 1; uiXR = uiXL + 1;
                }
            }

            pulLU = &pulMapArray[uiNrBins * (uiYU * uiNrX + uiXL)];
            pulRU = &pulMapArray[uiNrBins * (uiYU * uiNrX + uiXR)];
            pulLB = &pulMapArray[uiNrBins * (uiYB * uiNrX + uiXL)];
            pulRB = &pulMapArray[uiNrBins * (uiYB * uiNrX + uiXR)];
            Interpolate2D<Tdata, Tidx>(pImPointer,uiXRes,pulLU,pulRU,pulLB,pulRB,uiSubX,uiSubY,aLUT);
            pImPointer += uiSubX;			  /* set pointer on next matrix */
        }
        pImPointer += (uiSubY - 1) * uiXRes;
    }

    // de-alloc
    y_del<Tdata>(aLUT);
    y_del<Tidx>(pulMapArray);

    //
    return 0;						  /* return status OK */
}

template <class Tidx>
void ClipHistogram2D (Tidx* pulHistogram, Tidx uiNrGreylevels, Tidx ulClipLimit)
/* This function performs clipping of the histogram and redistribution of bins.
 * The histogram is clipped and the number of excess pixels is counted. Afterwards
 * the excess pixels are equally redistributed across the whole histogram (providing
 * the bin count is smaller than the cliplimit).
 */
{
    Tidx* pulBinPointer, *pulEndPointer, *pulHisto;
    Tidx ulNrExcess, ulUpper, ulBinIncr, ulStepSize, i;
    LONG64 lBinExcess;

    ulNrExcess = 0;  pulBinPointer = pulHistogram;

    for (i = 0; i < uiNrGreylevels; i++) { /* calculate total number of excess pixels */
        lBinExcess = (LONG64)pulBinPointer[i] - (LONG64)ulClipLimit;
        if (lBinExcess > 0) ulNrExcess += lBinExcess;	  /* excess in current bin */
    };


    /* Second part: clip histogram and redistribute excess pixels in each bin */
    ulBinIncr = ulNrExcess / uiNrGreylevels;		  /* average binincrement */
    ulUpper =  ulClipLimit - ulBinIncr;	 /* Bins larger than ulUpper set to cliplimit */

    for (i = 0; i < uiNrGreylevels; i++) {
        if (pulHistogram[i] > ulClipLimit) pulHistogram[i] = ulClipLimit; /* clip bin */
        else {
            if (pulHistogram[i] > ulUpper) {		/* high bin count */
                ulNrExcess -= pulHistogram[i] - ulUpper; pulHistogram[i]=ulClipLimit;
            }
            else {					/* low bin count */
                ulNrExcess -= ulBinIncr; pulHistogram[i] += ulBinIncr;
            }
        }
    }

    while (ulNrExcess) {   /* Redistribute remaining excess  */
        pulEndPointer = &pulHistogram[uiNrGreylevels]; pulHisto = pulHistogram;

        while (ulNrExcess && pulHisto < pulEndPointer) {
            ulStepSize = uiNrGreylevels / ulNrExcess;
            if (ulStepSize < 1) ulStepSize = 1;		  /* stepsize at least 1 */
            for (pulBinPointer=pulHisto; pulBinPointer < pulEndPointer && ulNrExcess;
                 pulBinPointer += ulStepSize) {
                if (*pulBinPointer < ulClipLimit) {
                    (*pulBinPointer)++;	 ulNrExcess--;	  /* reduce excess */
                }
            }
            pulHisto++;		  /* restart redistributing on other bin location */
        }
    }
}

template <class Tdata, class Tidx>
void MakeHistogram2D (Tdata* pImage, Tidx uiXRes,
                      Tidx uiSizeX, Tidx uiSizeY,
                      Tidx* &pulHistogram,
                      Tidx uiNrGreylevels, Tdata* pLookupTable)
/* This function classifies the greylevels present in the array image into
 * a greylevel histogram. The pLookupTable specifies the relationship
 * between the greyvalue of the pixel (typically between 0 and 4095) and
 * the corresponding bin in the histogram (usually containing only 128 bins).
 */
{
    Tdata* pImagePointer;
    unsigned int i;

    for (i = 0; i < uiNrGreylevels; i++) pulHistogram[i] = 0L; /* clear histogram */

    for (i = 0; i < uiSizeY; i++) {
        pImagePointer = &pImage[uiSizeX];
        while (pImage < pImagePointer) pulHistogram[pLookupTable[*pImage++]]++;
        pImagePointer += uiXRes;
        pImage = &pImagePointer[-uiSizeX];
    }
}

template <class Tdata, class Tidx>
void MapHistogram2D (Tidx* pulHistogram, Tdata Min, Tdata Max,
                     Tidx uiNrGreylevels, Tidx ulNrOfPixels)
/* This function calculates the equalized lookup table (mapping) by
 * cumulating the input histogram. Note: lookup table is rescaled in range [Min..Max].
 */
{
    Tidx i;
    Tidx ulSum = 0;
    REAL fScale = ((REAL)(Max - Min)) / ulNrOfPixels;
    Tidx ulMin = (Tidx)Min;

    for (i = 0; i < uiNrGreylevels; i++)
    {
        ulSum += pulHistogram[i];
        pulHistogram[i] = ulMin+ulSum*fScale;
        if (pulHistogram[i] > Max) pulHistogram[i] = Max;
    }
}

template <class Tdata, class Tidx>
void MakeLut2D (Tdata * pLUT, Tdata Min, Tdata Max, Tidx uiNrBins)
/* To speed up histogram clipping, the input image [Min,Max] is scaled down to
 * [0,uiNrBins-1]. This function calculates the LUT.
 */
{
    Tidx i;
    Tdata BinSize = (Tdata) (1 + (Max - Min) / uiNrBins);

    for (i = Min; i <= Max; i++)  pLUT[i] = (i - Min) / BinSize;
}

template <class Tdata, class Tidx>
void Interpolate2D (Tdata * pImage, Tidx uiXRes, Tidx * pulMapLU,
                    Tidx * pulMapRU, Tidx * pulMapLB,  Tidx * pulMapRB,
                    Tidx uiXSize, Tidx uiYSize, Tdata * pLUT)
/* pImage      - pointer to input/output image
 * uiXRes      - resolution of image in x-direction
 * pulMap*     - mappings of greylevels from histograms
 * uiXSize     - uiXSize of image submatrix
 * uiYSize     - uiYSize of image submatrix
 * pLUT	       - lookup table containing mapping greyvalues to bins
 * This function calculates the new greylevel assignments of pixels within a submatrix
 * of the image with size uiXSize and uiYSize. This is done by a bilinear interpolation
 * between four different mappings in order to eliminate boundary artifacts.
 * It uses a division; since division is often an expensive operation, I added code to
 * perform a logical shift instead when feasible.
 */
{
    Tidx uiIncr = uiXRes-uiXSize; /* Pointer increment after processing row */

    Tdata GreyValue;
    unsigned int uiNum = (unsigned int)uiXSize*(unsigned int)uiYSize; /* Normalization factor */

    unsigned int uiXCoef, uiYCoef, uiXInvCoef, uiYInvCoef, uiShift = 0;

    if (uiNum & (uiNum - 1))   /* If uiNum is not a power of two, use division */
        for (uiYCoef = 0, uiYInvCoef = uiYSize; uiYCoef < uiYSize; uiYCoef++, uiYInvCoef--,pImage+=uiIncr) {
            for (uiXCoef = 0, uiXInvCoef = uiXSize; uiXCoef < uiXSize; uiXCoef++, uiXInvCoef--) {
                GreyValue = pLUT[*pImage];		   /* get histogram bin value */
                *pImage++ = (Tdata)((uiYInvCoef * (uiXInvCoef*pulMapLU[GreyValue] + uiXCoef * pulMapRU[GreyValue])
                                      + uiYCoef * (uiXInvCoef * pulMapLB[GreyValue] + uiXCoef * pulMapRB[GreyValue])) / uiNum);
            }
        }
    else {			   /* avoid the division and use a right shift instead */
        while (uiNum >>= 1) uiShift++;		   /* Calculate 2log of uiNum */
        for (uiYCoef = 0, uiYInvCoef = uiYSize; uiYCoef < uiYSize; uiYCoef++, uiYInvCoef--,pImage+=uiIncr) {
            for (uiXCoef = 0, uiXInvCoef = uiXSize; uiXCoef < uiXSize; uiXCoef++, uiXInvCoef--) {
                GreyValue = pLUT[*pImage];	  /* get histogram bin value */
                *pImage++ = (Tdata)((uiYInvCoef * (uiXInvCoef * pulMapLU[GreyValue] + uiXCoef * pulMapRU[GreyValue])
                                     + uiYCoef * (uiXInvCoef * pulMapLB[GreyValue] + uiXCoef * pulMapRB[GreyValue])) >> uiShift);
            }
        }
    }
}
/*** end c codes for CLAHE ***/

template <class Tdata, class Tidx>
void contrastEnhancing(Tdata *&p, Tidx *dims, Tidx datatype, Tidx szblock, Tidx histbins, REAL maxslope)
{
    //
    // 5D CLAHE (XYZCT)
    //

    //
    if(!p || !dims )
    {
        cout<<"Invalid inputs for contrast enhancing function"<<endl;
        return;
    }

    //
    Tidx sx = dims[0];
    Tidx sy = dims[1];
    Tidx sz = dims[2];
    Tidx sc = dims[3];

    Tidx maxIntensity;
    Tdata minv=-1, maxv=-1;

    if(datatype == 1)
    {
        maxIntensity = 256;
        minv = (Tdata)0;
        maxv = (Tdata)255;
    }
    else if(datatype = 2)
    {
        maxIntensity = 4096;
        minv = (Tdata)0;
        maxv = (Tdata)4095;
    }
    else
    {
        cout<<"Contrast Enhancing function only supports 8-bit and 12-bit data"<<endl;
        return;
    }

    Tidx pagesz = sx*sy;
    Tidx pages = sz*sc;
    for(Tidx i=0; i<pages; i++)
    {
        Tdata *pImg = p + i*pagesz;

        //
        if(CLAHE2D<Tdata, Tidx>(pImg, sx, sy, minv, maxv, maxIntensity, szblock, szblock, histbins, maxslope))
        {
            cout<<"Fail to call contrast enhancing function"<<endl;
        }
    }

    //
    return;
}

template <class Tdata, class Tidx>
void guassianFiltering(Tdata *&p, Tidx *dims, Tidx *w)
{
    //
    // Gaussian filtering
    //

    //
    if(!p || !dims || !w)
    {
        cout<<"Invalid inputs for Gaussian Filtering function"<<endl;
        return;
    }

    //
    Tidx xdim, ydim, zdim;

    xdim=dims[0];
    ydim=dims[1];
    zdim=dims[2];

    Tidx ypage, zpage;

    ypage=xdim;
    zpage=xdim*ydim;

    Tidx wx, wy, wz;

    wx=w[0];
    wy=w[1];
    wz=w[2];

    if(wx<1 || wy<1 || wz<1 || wx>xdim || wy>ydim || wz>zdim)
    {
        cout<<"Invalid Gauss kernel size"<<endl;
        return;
    }

    //   Filtering aLONG64 x
    if(xdim<2)
    {
        //do nothing
    }
    else
    {
        //create Gaussian kernel
        Tdata  *weightsX = NULL;
        y_new<Tdata,Tidx>(weightsX, wx);

        Tidx half = wx >> 1;
        weightsX[half] = 1.;

        for (Tidx weight = 1; weight <= half; ++weight)
        {
            const Tdata  x = 3.* Tdata (weight) / Tdata (half);
            weightsX[half - weight] = weightsX[half + weight] = exp(-x * x / 2.);	// corresponding symmetric weightsX
        }

        Tdata k = 0.;
        for (Tidx weight = 0; weight < wx; ++weight)
            k += weightsX[weight];

        for (Tidx weight = 0; weight < wx; ++weight)
            weightsX[weight] /= k;

        //   Allocate 1-D extension array
        Tdata  *extension_bufferX = NULL;
        y_new<Tdata,Tidx>(extension_bufferX, xdim + (wx<<1));

        Tidx offset = wx>>1;
        const Tdata  *extStop = extension_bufferX + xdim + offset;

        for(Tidx iz = 0; iz < zdim; iz++)
        {
            for(Tidx iy = 0; iy < ydim; iy++)
            {
                Tdata  *extIter = extension_bufferX + wx;
                for(Tidx ix = 0; ix < xdim; ix++)
                {
                    *(extIter++) = p[iz*zpage + iy*ypage + ix];
                }

                // Extend image
                const Tdata  *const stop_line = extension_bufferX - 1;
                Tdata  *extLeft = extension_bufferX + wx - 1;
                const Tdata  *arrLeft = extLeft + 2;
                Tdata  *extRight = extLeft + xdim + 1;
                const Tdata  *arrRight = extRight - 2;

                while (extLeft > stop_line)
                {
                    *(extLeft--) = *(arrLeft++);
                    *(extRight++) = *(arrRight--);
                }

                // Filtering
                extIter = extension_bufferX + offset;

                Tdata  *resIter = p + iz*zpage + iy*ypage;

                while (extIter < extStop)
                {
                    Tdata sum = 0.;
                    const Tdata  *weightIter = weightsX;
                    const Tdata  *const End = weightsX + wx;
                    const Tdata * arrIter = extIter;
                    while (weightIter < End) sum += *(weightIter++) * Tdata (*(arrIter++));
                    extIter++;
                    *(resIter++) = sum;
                }
            }
        }

        //de-alloc
        y_del<Tdata>(weightsX);
        y_del<Tdata>(extension_bufferX);
    }

    //   Filtering aLONG64 y
    if(ydim<2)
    {
        //do nothing
    }
    else
    {
        //create Gaussian kernel
        Tdata  *weightsY = NULL;
        y_new<Tdata, Tidx>(weightsY, wy);

        Tidx half = wy >> 1;
        weightsY[half] = 1.;

        for (Tidx weight = 1; weight <= half; ++weight)
        {
            const Tdata  y = 3.* Tdata (weight) / Tdata (half);
            weightsY[half - weight] = weightsY[half + weight] = exp(-y * y / 2.);	// corresponding symmetric weightsY
        }

        Tdata k = 0.;
        for (Tidx weight = 0; weight < wy; ++weight)
            k += weightsY[weight];

        for (Tidx weight = 0; weight < wy; ++weight)
            weightsY[weight] /= k;

        //
        Tdata  *extension_bufferY = NULL;
        y_new<Tdata,Tidx>(extension_bufferY, ydim + (wy<<1));

        Tidx offset = wy>>1;
        const Tdata *extStop = extension_bufferY + ydim + offset;

        for(Tidx iz = 0; iz < zdim; iz++)
        {
            for(Tidx ix = 0; ix < xdim; ix++)
            {
                Tdata  *extIter = extension_bufferY + wy;
                for(Tidx iy = 0; iy < ydim; iy++)
                {
                    *(extIter++) = p[iz*zpage + iy*ypage + ix];
                }

                // Extend image
                const Tdata  *const stop_line = extension_bufferY - 1;
                Tdata  *extLeft = extension_bufferY + wy - 1;
                const Tdata  *arrLeft = extLeft + 2;
                Tdata  *extRight = extLeft + ydim + 1;
                const Tdata  *arrRight = extRight - 2;

                while (extLeft > stop_line)
                {
                    *(extLeft--) = *(arrLeft++);
                    *(extRight++) = *(arrRight--);
                }

                // Filtering
                extIter = extension_bufferY + offset;

                Tdata  *resIter = p + iz*zpage + ix;

                while (extIter < extStop)
                {
                    Tdata sum = 0.;
                    const Tdata  *weightIter = weightsY;
                    const Tdata  *const End = weightsY + wy;
                    const Tdata * arrIter = extIter;
                    while (weightIter < End) sum += *(weightIter++) * Tdata (*(arrIter++));
                    extIter++;
                    *resIter = sum;
                    resIter += ypage;
                }
            }
        }

        //de-alloc
        y_del<Tdata>(weightsY);
        y_del<Tdata>(extension_bufferY);
    }

    // Filtering aLONG64 z
    if(zdim<2)
    {
        //do nothing
    }
    else
    {
        //create Gaussian kernel
        Tdata  *weightsZ = NULL;
        y_new<Tdata, Tidx>(weightsZ, wz);

        Tidx half = wz >> 1;
        weightsZ[half] = 1.;

        for (Tidx weight = 1; weight <= half; ++weight)
        {
            const Tdata  z = 3.* Tdata (weight) / Tdata (half);
            weightsZ[half - weight] = weightsZ[half + weight] = exp(-z * z / 2.);	// corresponding symmetric weightsZ
        }

        Tdata k = 0.;
        for (Tidx weight = 0; weight < wz; ++weight)
            k += weightsZ[weight];

        for (Tidx weight = 0; weight < wz; ++weight)
            weightsZ[weight] /= k;

        //
        Tdata  *extension_bufferZ = NULL;
        y_new<Tdata, Tidx>(extension_bufferZ, zdim + (wz<<1));

        Tidx offset = wz>>1;
        const Tdata *extStop = extension_bufferZ + zdim + offset;

        for(Tidx iy = 0; iy < ydim; iy++)
        {
            for(Tidx ix = 0; ix < xdim; ix++)
            {
                Tdata  *extIter = extension_bufferZ + wz;
                for(Tidx iz = 0; iz < zdim; iz++)
                {
                    *(extIter++) = p[iz*zpage + iy*ypage + ix];
                }

                // Extend image
                const Tdata  *const stop_line = extension_bufferZ - 1;
                Tdata  *extLeft = extension_bufferZ + wz - 1;
                const Tdata  *arrLeft = extLeft + 2;
                Tdata  *extRight = extLeft + zdim + 1;
                const Tdata  *arrRight = extRight - 2;

                while (extLeft > stop_line)
                {
                    *(extLeft--) = *(arrLeft++);
                    *(extRight++) = *(arrRight--);
                }

                // Filtering
                extIter = extension_bufferZ + offset;

                Tdata  *resIter = p + iy*ypage + ix;

                while (extIter < extStop)
                {
                    Tdata sum = 0.;
                    const Tdata  *weightIter = weightsZ;
                    const Tdata  *const End = weightsZ + wz;
                    const Tdata * arrIter = extIter;
                    while (weightIter < End) sum += *(weightIter++) * Tdata (*(arrIter++));
                    extIter++;
                    *resIter = sum;
                    resIter += zpage;
                }
            }
        }

        //de-alloc
        y_del<Tdata>(weightsZ);
        y_del<Tdata>(extension_bufferZ);
    }

    return;
}

template <class Tdata, class Tidx>
void thresholding(Tdata *&p, Tidx *dims)
{
    //
    // threshold segmentation using k-means
    //

    //
    if(!p || !dims)
    {
        cout<<"Invalid inputs for thresholding function"<<endl;
        return;
    }

    //
    Tidx xdim, ydim, zdim;

    xdim=dims[0];
    ydim=dims[1];
    zdim=dims[2];

    Tidx pagesz;
    pagesz=xdim*ydim*zdim;

    //
    Tidx BINS = 1024; // histogram bins

    Tidx *h=NULL, *hc=NULL;
    y_new<Tidx, Tidx>(h, BINS);
    y_new<Tidx, Tidx>(hc, BINS);

    memset(h, 0, sizeof(Tidx)*BINS);

    // histogram
    HistogramLUT<Tdata, Tidx> hlut;
    hlut.initLUT(p, pagesz, BINS);
    foreach(pagesz, i)
    {
        h[hlut.getIndex(p[i])] ++;
    }

    // heuristic init center
    REAL mub=0.05*(hlut.maxv - hlut.minv) + hlut.minv;
    REAL muf=0.30*(hlut.maxv - hlut.minv) + hlut.minv;

    //
    while (true)
    {
        REAL oldmub=mub, oldmuf=muf;

        for(Tidx i=0; i<BINS; i++)
        {
            if(h[i]==0)
                continue;

            REAL cb = y_abs<REAL>(REAL(hlut.lut[i])-mub);
            REAL cf = y_abs<REAL>(REAL(hlut.lut[i])-muf);

            hc[i] = (cb<=cf)?1:2; // class 1 and class 2
        }

        // update centers
        REAL sum_b=0, sum_bw=0, sum_f=0, sum_fw=0;

        for(Tidx i=0; i<BINS; i++)
        {
            if(h[i]==0)
                continue;

            if(hc[i]==1)
            {
                sum_bw += (i+1)*h[i];
                sum_b += h[i];
            }
            else if(hc[i]==2)
            {
                sum_fw += (i+1)*h[i];
                sum_f += h[i];
            }
        }

        mub = hlut.lut[ Tidx(sum_bw/sum_b) ];
        muf = hlut.lut[ Tidx(sum_fw/sum_f) ];

        if(y_abs<REAL>(mub - oldmub)<1 && y_abs<REAL>(muf - oldmuf)<1)  break;
    }

    //
    Tdata threshold = (mub+muf)/2;
    for(Tidx i=0; i<pagesz; i++)
    {
        if(p[i]>threshold)
            p[i] = hlut.maxv;
        else
            p[i] = hlut.minv;
    }

    //
    y_del<Tidx>(h);
    y_del<Tidx>(hc);

    //
    return;
}

template <class Tdata, class Tidx>
void rescaling(Tdata *&p, Tidx totalplxs, Tdata lv, Tdata rv)
{
    //
    // rescale intensity to [lv,rv]
    //

    //
    if(!p || rv<lv)
    {
        cout<<"Invalid inputs for rescaling function"<<endl;
        return;
    }

    //
    REAL max_v = -INF, min_v = INF;

    foreach(totalplxs, i)
    {
        max_v = (p[i]>max_v)? p[i] : max_v;
        min_v = (p[i]<min_v)? p[i] : min_v;
    }
    max_v -= min_v;

    if(y_abs<REAL>(max_v)<EPS)
    {
        cout<<"Data not supported!"<<endl;
        return;
    }

    foreach(totalplxs, i)
    {
        p[i]=(rv-lv)*((REAL)(p[i]) - min_v)/max_v + lv;
    }

    return;
}

template <class Tdata, class Tidx>
void normalizing(Tdata *&p, Tidx totalplxs, Tdata lv, Tdata rv)
{
    //
    // normalize and scale intensity to [lv,rv]
    //

    //
    if(!p || rv<lv)
    {
        cout<<"Invalid inputs for normalizing function"<<endl;
        return;
    }

    //
    REAL mean_v = 0, std_v = 0;

    foreach(totalplxs, i)
    {
        mean_v += (REAL)(p[i]);
    }
    mean_v /= (REAL)totalplxs;

    foreach(totalplxs, i)
    {
        REAL val = (REAL)(p[i]) - mean_v;
        std_v += val*val;
    }
    std_v = sqrt(std_v);

    if(y_abs<REAL>(std_v)<EPS )
    {
        cout<<"Error standard deviation"<<endl;
        return;
    }

    foreach(totalplxs, i)
    {
        p[i] = ((REAL)(p[i]) - mean_v)/std_v; // real value!
    }

    //
    rescaling(p, totalplxs, lv, rv);

    //
    return;
}

template <class Tdata, class Tidx>
void gradientMagnitude(Tdata *&p, Tidx *dims, Tidx gstep = 1)
{
    //
    // extract gradient magnitude
    //

    //
    if(!p || !dims)
    {
        cout<<"Invalid inputs for gradient magnitude function"<<endl;
        return;
    }

    //
    Tidx xdim, ydim, zdim;

    xdim=dims[0];
    ydim=dims[1];
    zdim=dims[2];

    Tidx zoffsets = xdim*ydim;
    Tidx yoffsets = xdim;

    Tidx pagesz = zdim*zoffsets;

    Tdata *pData = NULL;
    y_new<Tdata, Tidx>(pData, pagesz);

    for(Tidx i=0; i<pagesz; i++)
    {
        pData[i] = p[i];
    }

    //
    REAL normfactor = -INF;
    for(Tidx z=0;z<zdim;z++)
    {
        Tidx ofz = z*zoffsets;
        for(Tidx y=0;y<ydim;y++)
        {
            Tidx ofy = ofz + y*xdim;
            for(Tidx x=0;x<xdim;x++)
            {
                Tidx idx = ofy + x;

                //
                Tidx xs, xe;
                xs = x-gstep; if(xs<0) xs=0;
                xe = x+gstep; if(xe>=xdim) xe=xdim-1;

                Tidx ys, ye;
                ys = y-gstep; if(ys<0) ys=0;
                ye = y+gstep; if(ye>=ydim) ye=ydim-1;

                Tidx zs, ze;
                zs = z-gstep; if(zs<0) zs=0;
                ze = z+gstep; if(ze>=zdim) ze=zdim-1;

                //
                REAL vxl = pData[ z*zoffsets + y*yoffsets + xs ];
                REAL vxr = pData[ z*zoffsets + y*yoffsets + xe ];

                REAL vyu = pData[ z*zoffsets + ys*yoffsets + x ];
                REAL vyd = pData[ z*zoffsets + ye*yoffsets + x ];

                REAL vzf = pData[ zs*zoffsets + y*yoffsets + x ];
                REAL vzb = pData[ ze*zoffsets + y*yoffsets + x ];

                REAL Ix = vxr - vxl;
                REAL Iy = vyd - vyu;
                REAL Iz = vzb - vzf;

                //
                REAL val = sqrt(Ix*Ix + Iy*Iy + Iz*Iz);

                if(val > normfactor) normfactor = val;

                //
                p[idx] = val;

            }
        }
    }

    foreach(pagesz, i)
    {
        p[i] /= normfactor;
    }

    // de-alloc
    y_del<Tdata>(pData);

    //
    return;
}

template <class Tdata, class Tidx>
void extractsurfaces(Tdata *&p, Tidx *dims)
{
    //
    // extract surfaces from the binary image
    //

    // input: [0, 1] binary image
    REAL thresh = 0.5;

    //
    if(!p || !dims)
    {
        cout<<"Invalid inputs for extract surfaces function"<<endl;
        return;
    }

    //
    Tidx xdim, ydim, zdim;

    xdim=dims[0];
    ydim=dims[1];
    zdim=dims[2];

    Tidx zoffsets = xdim*ydim;
    Tidx yoffsets = xdim;

    Tidx pagesz = zdim*zoffsets;

    Tdata *pData = NULL;
    y_new<Tdata, Tidx>(pData, pagesz);

    foreach(pagesz, i)
    {
        pData[i] = p[i];
        p[i] = 0;
    }

    //
    Tidx neighbor3d[6] = {-1, 1, -yoffsets, yoffsets, -zoffsets, zoffsets};
    Tidx neighbor2d[4] = {-1, 1, -yoffsets, yoffsets};

    //
    for(Tidx z=0;z<zdim;z++)
    {
        Tidx ofz = z*zoffsets;
        for(Tidx y=0;y<ydim;y++)
        {
            Tidx ofy = ofz + y*xdim;
            for(Tidx x=0;x<xdim;x++)
            {
                Tidx idx = ofy + x;

                //
                if(zdim<2)
                {
                    if(x==0 || x==xdim-1 || y==0 || y==ydim-1)
                        continue;
                }
                else
                {
                    if(x==0 || x==xdim-1 || y==0 || y==ydim-1 || z==0 || z==zdim-1)
                        continue;
                }
                //

                if( (REAL)(pData[idx])>thresh )
                {
                    if(zdim<2)
                    {
                        foreach(4, ineighbor)
                        {
                            if( (REAL)(pData[ idx + neighbor2d[ineighbor] ]) < thresh)
                                p[idx] = 1;
                        }
                    }
                    else
                    {
                        foreach(6, ineighbor)
                        {
                            if( (REAL)(pData[ idx + neighbor3d[ineighbor] ]) < thresh)
                                p[idx] = 1;
                        }
                    }
                }

            }
        }
    }

    // de-alloc
    y_del<Tdata>(pData);

    //
    return;
}

template <class Tdata, class Tidx>
void flip(Tdata *&pOut, Tdata *pIn, Tidx *dims, AxisType atype)
{
    //
    // flip image aLONG64 x, y, z, c, or t
    //

    //
    if(!pIn || !dims)
    {
        cout<<"Invalid inputs for flip function"<<endl;
        return;
    }

    //
    Tidx xdim, ydim, zdim;

    xdim=dims[0];
    ydim=dims[1];
    zdim=dims[2];

    Tidx zoffsets = xdim*ydim;
    Tidx yoffsets = xdim;

    Tidx pagesz = zdim*zoffsets;

    if(!pOut)   y_new<Tdata, Tidx>(pOut, pagesz);

    if(atype==AX)
    {
        // flip aLONG64 x axis
        foreach(zdim, k)
        {
            Tidx offset_k = k*zoffsets;
            foreach(ydim, j)
            {
                Tidx offset_j = j*yoffsets + offset_k;
                foreach(xdim, i)
                {
                    pOut[offset_j + xdim - 1 - i] = pIn[offset_j + i];
                }
            }
        }
    }
    else if(atype==AY)
    {
        // flip aLONG64 y axis
        foreach(zdim, k)
        {
            Tidx offset_k = k*zoffsets;
            foreach(ydim, j)
            {
                Tidx offset_j = j*yoffsets + offset_k;
                Tidx offset_j_o = (ydim-1-j)*yoffsets + offset_k;
                foreach(xdim, i)
                {
                    pOut[offset_j_o + i] = pIn[offset_j + i];
                }
            }
        }
    }
    else if(atype==AZ)
    {
        // flip aLONG64 z axis
        foreach(zdim, k)
        {
            Tidx offset_k = k*zoffsets;
            Tidx offset_k_o = (zdim-1-k)*zoffsets;
            foreach(ydim, j)
            {
                Tidx offset_j = j*yoffsets + offset_k;
                Tidx offset_j_o = j*yoffsets + offset_k_o;
                foreach(xdim, i)
                {
                    pOut[offset_j_o + i] = pIn[offset_j + i];
                }
            }
        }
    }
    else
    {
        cout<<"unknown axis type to flip image"<<endl;
        return;
    }

    //
    return;
}

template <class Tdata, class Tidx>
void imgcopy(Tdata *&pOut, Tdata *pIn, Tidx totalplxs)
{
    //
    // image copy
    //

    //
    if(!pIn || !pOut)
    {
        cout<<"Invalid inputs for image copy function"<<endl;
        return;
    }

    //
    foreach(totalplxs, i)
        pOut[i] = pIn[i];

    //
    return;
}

// tps basic function
template <class Tdata>
Tdata tpsRBF(Tdata r2)
{
    if(r2 == 0.0)
        return 0.0;
    else
        return (0.5*r2*log(r2));
}

// image intersection
template <class Tdata, class Tidx>
void imgintersection(Tdata *&pOut, Tdata *p1, Tdata *p2, Tidx totalplxs, Tidx method, Tdata ceilingval)
{
    //
    // method 0: min; 1: geometric mean; 2: product
    // ceiling value: 255 for 8-bit data; 4095 for 12-bit data
    //

    if(!p1 || !p2)
    {
        cout<<"Invalide inputs for image intersection"<<endl;
        return;
    }

    if(!pOut) y_new<Tdata, Tidx>(pOut, totalplxs);

    if(method==0)
    {
        foreach(totalplxs, i)
        {
            pOut[i] = y_min<Tdata>(p1[i], p2[i]);
        }
    }
    else if(method==1)
    {
        foreach(totalplxs, i)
        {
            pOut[i] = (Tdata)sqrt(double(p1[i])*double(p2[i]));
        }
    }
    else if(method==2)
    {
        REAL minv = INF, maxv = -INF;
        foreach(totalplxs, i)
        {
            REAL val = REAL(p1[i])*REAL(p2[i]);

            if(val<minv) minv = val;
            if(val>maxv) maxv = val;
        }
        maxv -= minv;

        foreach(totalplxs, i)
        {
            pOut[i] = (Tdata)((REAL(p1[i])*REAL(p2[i]))/maxv)*ceilingval;
        }
    }
    else
    {
        cout<<"Invalide inputs for image intersection"<<endl;
        return;
    }

    return;
}

#endif // __CVISUTILITY_HPP
